#!/usr/bin/env bash
# Script to run ROVIOLI from a Euroc rosbag data source with localization enabled.
# Usage: tutorial_euroc <localization map path> <output save folder> <rosbag path> [<additional rovioli flags>]

LOCALIZATION_MAP_INPUT=$1
LOCALIZATION_MAP_OUTPUT=$2
ROSBAG=$3
NCAMERA_CALIBRATION="$ROVIO_CONFIG_DIR/ncamera-euroc.yaml"
IMU_PARAMETERS_MAPLAB="$ROVIO_CONFIG_DIR/imu-adis16488.yaml"
IMU_PARAMETERS_ROVIO="$ROVIO_CONFIG_DIR/imu-sigmas-rovio.yaml"
REST=$@

rosrun rovioli rovioli \
  --alsologtostderr=1 \
  --v=2 \
  --ncamera_calibration=$NCAMERA_CALIBRATION  \
  --imu_parameters_maplab=$IMU_PARAMETERS_MAPLAB \
  --imu_parameters_rovio=$IMU_PARAMETERS_ROVIO \
  --publish_debug_markers  \
  --datasource_type="rosbag" \
  --optimize_map_to_localization_map=false \
  --vio_localization_map_folder=$LOCALIZATION_MAP_INPUT \
  --save_map_folder=$LOCALIZATION_MAP_OUTPUT \
  --map_builder_save_image_as_resources=false \
  --datasource_rosbag=$ROSBAG $REST
